﻿#region Includes
using System;
using OpenTK;
#endregion

namespace GingerGL.Math
{
    /// <summary>
    /// Plane Structure
    /// </summary>
    [Serializable]
    public struct Plane : IEquatable<Plane>
    {
        #region Variables
        static public readonly int SizeInBytes = sizeof(float) + Vector3.SizeInBytes;
        static public readonly Plane Up = new Plane(0, 1, 0, 0);
        static public readonly Plane Down = new Plane(0, -1, 0, 0);
        static public readonly Plane Empty = new Plane(0, 0, 0, 0);
        /// <summary>
        /// Direction the plane is facing
        /// </summary>
        public Vector3 Normal;
        /// <summary>
        /// Gets Normal X
        /// </summary>
        public float A { get { return Normal.X; } }
        /// <summary>
        /// Gets Normal Y
        /// </summary>
        public float B { get { return Normal.Y; } }
        /// <summary>
        /// Gets Normal Z
        /// </summary>
        public float C { get { return Normal.Z; } }
        /// <summary>
        /// Distance from Normal
        /// </summary>
        public float D;
        #endregion 

        #region Constructors
        /// <summary>
        /// Initialize a new instance of object
        /// </summary>
        public Plane(Vector3 a, Vector3 b, Vector3 c)
        {
            Vector3 ab = b - a;
            Vector3 ac = c - a;
            Vector3 cross = Vector3.Cross(ab, ac);
            Normal = Vector3.Normalize(cross);
            D = -(Vector3.Dot(cross, a));
        }
        /// <summary>
        /// Initialize a new instance of object
        /// </summary>
        public Plane(Vector4 value) : this(new Vector3(value.X, value.Y, value.Z), value.W) { }
        /// <summary>
        /// Initialize a new instance of object
        /// </summary>
        public Plane(float a, float b, float c, float d) : this(new Vector3(a, b, c), d) { }
        /// <summary>
        /// Initialize a new instance of object
        /// </summary>
        public Plane(Vector3 normal, float intercept) { Normal = normal; D = intercept; }
        #endregion

        #region Methods

        #region Distance
        public float Distance(Vector3 point) { return PerpendicularDistance(point, this); }
        public float Distance(BoundingSphere sphere) { return PerpendicularDistance(sphere.Center, this) - sphere.Radius; }
        #endregion

        #region Verified
        /// <summary>
        /// Gets Intersection Type of Point
        /// </summary>
        public PlaneIntersectionType Intersects(Vector3 point)
        {
            float dist = ClassifyPoint(point, this);
            if (dist > MathHelper2.DefaultTolerance) return PlaneIntersectionType.Front;
            if (dist < -MathHelper2.DefaultTolerance) return PlaneIntersectionType.Back;
            return PlaneIntersectionType.Intersecting;
        }
        /// <summary>
        /// Indicating which side (positive/negative) of a plane a point is on.
        /// Returns > 0 if on the positive side, lt 0 if on the negative size, 0 if on the plane.
        /// </summary>
        static internal float ClassifyPoint(Vector3 point, Plane plane)
        {
            return point.X * plane.Normal.X + point.Y * plane.Normal.Y + point.Z * plane.Normal.Z + plane.D;
        }
        /// <summary>
        /// Normalize Plane
        /// </summary>
        public void Normalize()
        {
            float normalLength = 1 / Normal.Length;
            Normal *= normalLength;
            D *= normalLength;
        }
        #endregion
        /// <summary>
        /// Gets the vector of 3 crossing planes
        /// </summary>
        static public Vector3 IntersectionPoint(Plane a, Plane b, Plane c)
        {
            // Formula used
            //                d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
            //P =   -------------------------------------------------------------------------
            //                             N1 . ( N2 * N3 )
            //
            // Note: N refers to the normal, d refers to the displacement. '.' means dot product. '*' means cross product

            Vector3 v1, v2, v3;
            float f = -Vector3.Dot(a.Normal, Vector3.Cross(b.Normal, c.Normal));

            v1 = (a.D * (Vector3.Cross(b.Normal, c.Normal)));
            v2 = (b.D * (Vector3.Cross(c.Normal, a.Normal)));
            v3 = (c.D * (Vector3.Cross(a.Normal, b.Normal)));

            Vector3 vec = new Vector3(v1.X + v2.X + v3.X, v1.Y + v2.Y + v3.Y, v1.Z + v2.Z + v3.Z);
            return vec / f;
        }
        
        //find point of intersection of 3 planes
        public bool Intersect3(Plane p2, Plane p3, out Vector3 result)
        {
            result = new Vector3();
            float denominator = Vector3.Dot(Normal, Vector3.Cross(p2.Normal, p3.Normal));
            //scalar triple product of normals
            if (denominator == 0.0f)									//if zero
                return false;										//no intersection

            Vector3 temp1, temp2, temp3;
            temp1 = Vector3.Cross(p2.Normal, p3.Normal) * D;
            temp2 = Vector3.Cross(p3.Normal, Normal) * p2.D;
            temp3 = Vector3.Cross(Normal, p2.Normal) * p3.D;
            result = (temp1 + temp2 + temp3) / (-denominator);
            return true;
        }
      
        // Calculates the perpendicular distance from a point to a plane
        internal static float PerpendicularDistance(Vector3 point, Plane plane)
        {
            // dist = (ax + by + cz + d) / sqrt(a*a + b*b + c*c)
            return (float)System.Math.Abs((plane.Normal.X * point.X +
                                           plane.Normal.Y * point.Y +
                                           plane.Normal.Z * point.Z) /
                                          System.Math.Sqrt(plane.Normal.X * plane.Normal.X +
                                                           plane.Normal.Y * plane.Normal.Y +
                                                           plane.Normal.Z * plane.Normal.Z));
        }
        public Plane Lerp(Plane p2, float factor)
        {
            Plane result;
            result.Normal = Normal * (1.0f - factor) + p2.Normal * factor;
            result.Normal.Normalize();
            result.D = D * (1.0f - factor) + p2.D * factor;
            return result;
        }
        

        #region Object Overrides
        /// <summary>
        /// Equals override
        /// </summary>
        public override bool Equals(object other) { return (other is Plane) ? this.Equals((Plane)other) : false; }
        /// <summary>
        /// Gets hash code
        /// </summary>
        public override int GetHashCode() { return Normal.GetHashCode() ^ D.GetHashCode(); }
        /// <summary>
        /// Plane to string
        /// </summary>
        public override string ToString() { return string.Format("Distance:{1} Normal:{0}", Normal, D); }
        #endregion

        #region IEquatable Overrides
        /// <summary>
        /// IEquatable Implementaion
        /// </summary>
        public bool Equals(Plane other) { return ((Normal == other.Normal) && (D == other.D)); }
        #endregion

        #region Operators
        static public bool operator !=(Plane plane1, Plane plane2) { return !plane1.Equals(plane2); }
        static public bool operator ==(Plane plane1, Plane plane2) { return plane1.Equals(plane2); }
        static public Plane operator -(Plane left) { return new Plane(-left.Normal, -left.D); }
        #endregion

        #endregion
    }
}
